﻿using System;
using System.Diagnostics;
using System.Drawing;
using System.Windows.Forms;
using Microsoft.Dss.ServiceModel.Dssp;
using Microsoft.Robotics.Services.IRobot.Lite.Proxy;
using Microsoft.Robotics.Services.Sensors.Kinect.Proxy;
using NestedParticleFilter;
using Point = NestedParticleFilter.Point;

namespace MatLocalization
{
    public partial class ImageForm : Form
    {
        
        private readonly KinectOperations _kinectPort;
        private readonly IRobotLiteOperations _drivePort;
        private readonly MatLocalizationOperations _mainPort;
        delegate void SetPoseCallback(RobotPose pose);
        delegate void SetPointCallback(Point  point);
        delegate void SetLandmarkCallback(string name);
        delegate void SetProgress(int value, int c);
        private delegate void SetBitmap(Image img);
        
        public void  SetBoydPicForI(Image image)
        {
             if (boydPicBoxForI.InvokeRequired)
            {
                var d = new SetBitmap(SetBoydPicForI);
                Invoke(d, new object[] {image});
            }
             else
             {
                 boydPicBoxForI.Image = image;
             }
            
        }
        public void SetBoydPicFoJ(Image image)
        {
            if (boydPicBoxForJ.InvokeRequired)
            {
                var d = new SetBitmap(SetBoydPicFoJ);
                Invoke(d, new object[] {image});
            }
            else
            {
                boydPicBoxForJ.Image = image;
            }
           
        }
        
        public void SetKinectPic(Image image)
        {
           if (kinectPicBox.InvokeRequired)
           {

               var d = new SetBitmap(SetKinectPic);
               Invoke(d, new object[] {image});
           }
           else
            {
                kinectPicBox.Image = image;
            }
        }
        public void SetDepthPic (Image image)
        {
             if (depthPicBox.InvokeRequired)
           {

               var d = new SetBitmap(SetDepthPic);
               Invoke(d, new object[] {image});
           }
           else
            {
                depthPicBox.Image = image;
            }
           
        }

        public ImageForm(MatLocalizationOperations mainPort, KinectOperations kinectKinectPort, IRobotLiteOperations drivePort)
        {
            _kinectPort = kinectKinectPort;
            _drivePort = drivePort;
            _mainPort = mainPort;
            InitializeComponent();
        }
        public void UpdateTarget(Point point)
        {
            SetTarget(point);
        }

        public void UpdateLastObservation(string  name)
        {
            SetObservation(name);

        }
        public void UpdatePatAveragePose(RobotPose pose)
        {
            SetPatAveragePose(pose);
        }


        /// <summary>
        /// Changing the pat's real pose text safely
        /// </summary>
        private void SetTarget(Point point)
        {

            if (txtTarget.InvokeRequired)
            {
                var d = new SetPointCallback(SetTarget);
                Invoke(d, new object[] { point });

            }
            else
            {
                if (point == null)
                    txtTarget.Text = String.Empty;
                else
                    txtTarget.Text = point.X.ToString("##.##")
                    + "\t" + point.Z.ToString("##.##") + "\t" ;

            }
        }
        private void SetObservation(string name)
        {
            if (txtLastObs.InvokeRequired)
            {
                var d = new SetLandmarkCallback(SetObservation);
                Invoke(d, new object[] { name });

            }
            else
            {
                txtLastObs.Text = name;

            }
        }

        public void UpdateProgressBar (int value, int capacity)
        {
            if (progressBattery.InvokeRequired)
            {
                var d = new SetProgress(UpdateProgressBar);
                Invoke(d, new object[] {value, capacity});
            }
            else
            {
                progressBattery.Maximum = capacity;
                progressBattery.Value = value;
            }
        }

        private void SetPatAveragePose(RobotPose pose)
        {
            if (txtAvgPose.InvokeRequired)
            {
                var d = new SetPoseCallback(SetPatAveragePose);
                Invoke(d, new object[] { pose });

            }
            else
            {
                txtAvgPose.Text = pose.X.ToString("##.##")
                    + "\t" + pose.Z.ToString("##.##") + "\t" + pose.Theta.ToString("##.##");
                 
            }
        }

        private void BtnStopOobooClick(object sender, EventArgs e)
        {
            _drivePort.CreateDriveDirect(0,0);
           
        }

        private void BtnStartOobooClick(object sender, EventArgs e)
        {
            _mainPort.Post(new MatExplore());

        }

        private void BtnSetClick(object sender, EventArgs e)
        {

            int angle = 0;
            bool success = int.TryParse(angleTxtBox.Text, out angle);

            if (success && angle <= 27 && angle >= -27)
            {
                var tiltRequest = new UpdateTiltRequest {Tilt = angle};
                _kinectPort.UpdateTilt(tiltRequest);

            }

        }
        
      


        private void SetVelocityBtmClick(object sender, EventArgs e)
        {
            try
            {
                int speed;
                bool success = int.TryParse(velocityTxtBox.Text, out speed);

                if (success)
                {
                    MatLocalizationService.ForwardVelocity = speed;
                    MatLocalizationService.RotationVelocity = speed - 50;
                    MatLocalizationService.RotationDegree = MatLocalizationService.RotationVelocity - 50;
                }
                
               
            }
            catch(Exception exception)
            {
                Debug.WriteLine(exception.ToString());
            }
        }

        private void TerminateBtmClick(object sender, EventArgs e)
        {
            _drivePort.CreateDriveDirect(0, 0);
            
            _mainPort.Post(new Terminate());
        }

        private void KinectPicBoxClick(object sender, EventArgs e)
        {

        }

        private void button1_Click(object sender, EventArgs e)
        {
            MatLocalizationService._jTagged = true;
        }

        

       

       
       
    }
}
